/*****************************************************************************/
/*                                                                           */
/*    Domino Operation System Kernel Module                                  */
/*                                                                           */
/*    Copyright (C) 2010 Laszlo Arvai                                        */
/*                                                                           */
/*    ------------------------------------------------------------------   */
/*    GPS UART Kernel Driver                                                 */
/*****************************************************************************/

///////////////////////////////////////////////////////////////////////////////
// Includes
#include <krnlTypes.h>
#include <drvUART.h>
#include <krnlString.h>
#include <krnlAsciiCodes.h>
#include <krnlSystemTimer.h>
#include <drvGPS.h>

/*****************************************************************************/
/* Constants                                                                 */
/*****************************************************************************/
#define drvGPS_SENTENCE_START '$'
#define drvGPS_CHECKSUM_START '*'
#define drvGPS_SEPARATOR ','

#define drvGPS_RX_BUFFER_LENGTH 16

#define drvGPS_COMMUNICATION_TIMEOUT 1500

/*****************************************************************************/
/* Types                                                                     */
/*****************************************************************************/
typedef enum
{
	drvGPSRS_Idle,
	drvGPSRS_Sentence,
	drvGPSRS_Checksum1,
	drvGPSRS_Checksum2

} drvGPSRxStatus;
													
static dosConstString l_sentences[] =
{
	"GPGGA",
	"GPRMC",
	DOS_NULL
};

#define drvGPS_IDX_GPGGA 0
#define drvGPS_IDX_GPRMC 1

/*****************************************************************************/
/* Module global variables                                                   */
/*****************************************************************************/
static drvGPSRxStatus l_rx_status = drvGPSRS_Idle;

static dosChar l_rx_buffer[drvGPS_RX_BUFFER_LENGTH+1];
static dosByte l_rx_buffer_pos;
static dosByte l_sentence_index;
static dosByte l_param_index;
static dosByte l_checksum;
static dosByte l_received_cheksum;
//static drvGPSData l_last_data;
static drvGPSData l_current_data;

static dosByte l_error_code = drvGPS_COMMUNICATION_ERROR;
//static dosWord l_last_communication_timestamp = 0;

/*****************************************************************************/
/* Local functions                                                           */
/*****************************************************************************/
static dosByte drvGPSConvertTwoDigitNumber(dosStringLength* inout_buffer_pos, dosBool* inout_success);
static void drvGPSParseBuffer(void);
dosInt32 drvGPSConvertDegree(dosStringLength* inout_buffer_pos, dosBool* inout_success);

///////////////////////////////////////////////////////////////////////////////
// UART Initialize
void drvGPSInit(void)
{
	// init
	l_rx_status= drvGPSRS_Idle;
	l_error_code = drvGPS_COMMUNICATION_ERROR;

	// UART init
	drvGPSUARTInit();
}	

/*****************************************************************************/
/* Receiver section                                                          */
/*****************************************************************************/

///////////////////////////////////////////////////////////////////////////////
// Character received interrupt callback
void drvGPSUARTReceiveInterrupt( dosChar in_char )
{
	switch(l_rx_status)
	{
		// waiting for sentence start
		case drvGPSRS_Idle:
			if(in_char == drvGPS_SENTENCE_START)
			{
				l_param_index = 0;
				l_rx_buffer_pos = 0;
				l_rx_status= drvGPSRS_Sentence;
				l_checksum = 0;
			}
			break;

		// receiving sentence characters
		case drvGPSRS_Sentence:
			switch(in_char)
			{
				// sentence end without checksum
				case ASCII_LF:
				case ASCII_CR:
					break;

				// sentence end with checksum
				case drvGPS_CHECKSUM_START:
					l_received_cheksum = 0;
					l_rx_status= drvGPSRS_Checksum1;
					break;

				// parse buffer content at the separator
				case drvGPS_SEPARATOR:
					l_checksum ^= in_char;
					drvGPSParseBuffer();
					break;

				default:
					l_checksum ^= in_char;
					if(l_rx_buffer_pos < drvGPS_RX_BUFFER_LENGTH)
						l_rx_buffer[l_rx_buffer_pos++] = in_char;
					break;
			}
			break;

		// receiving checksum
		case drvGPSRS_Checksum2:
		case drvGPSRS_Checksum1:
		{
			dosBool success = dosTrue;

			l_received_cheksum = (l_received_cheksum << 4) + strHexDigitToNibble(in_char, &success);
			if(success)
			{
				if(l_rx_status == drvGPSRS_Checksum2)
				{
					if(l_received_cheksum == l_checksum)
					{
					}
					else
					{
						success = dosFalse;
					}
				}
				else
				{
					l_rx_status = drvGPSRS_Checksum2;
				}
			}
		}
		break;
	}
}

#define GENERATE_CASE_ID(sentence,param) ((sentence << 5) + param)

static void drvGPSParseBuffer(void)
{
	dosByte i;
	dosBool success;
	dosStringLength pos;

	// terminate buffer
	l_rx_buffer[l_rx_buffer_pos] = '\0';

	// parse content
	if(l_param_index == 0)
	{
		i = 0;
		while(l_sentences[i] != DOS_NULL && strCompareConstStringNoCase(l_rx_buffer, l_sentences[i]) != 0)
			i++;

		if(l_sentences[i] == DOS_NULL)
		{
			// skip unknown sentences
			l_rx_status = drvGPSRS_Idle;
		}
		else
		{
			// sentence was found -> continue parsing
			l_sentence_index = i;
			l_param_index++;
			l_rx_buffer_pos = 0;
		}
	}
	else
	{
		// init
		success = dosTrue;
		pos = 0;

		// parse params
		switch(GENERATE_CASE_ID(l_sentence_index, l_param_index))
		{
			// GPGGA and GPRMC first parameter (UTC time)
			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA,1):
			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC,1):
				success = dosTrue;
				pos = 0;

				// convert time
				l_current_data.UTCDateTime.Hour = drvGPSConvertTwoDigitNumber(&pos, &success);
				l_current_data.UTCDateTime.Minute = drvGPSConvertTwoDigitNumber(&pos, &success);
				l_current_data.UTCDateTime.Second = drvGPSConvertTwoDigitNumber(&pos, &success);

				// next character should be 'dot'
				if(l_rx_buffer[pos] == '.')
					pos++;
				else
					success = dosFalse;

				// convert millisecond
				strStringToWord(l_rx_buffer, drvGPS_RX_BUFFER_LENGTH, &pos, &success, &l_current_data.UTCMillisecond);

				break;

			// GPRMC validity
			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC,2):
				if(l_rx_buffer[0] == 'A')
					l_current_data.Status = drvGPS_OK;
				else											
					l_current_data.Status = drvGPS_NO_FIX;
				break;

			// Latitude
			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC, 3): 
			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA, 2):
				l_current_data.Latitude = drvGPSConvertDegree(&pos, &success);
				break;

			// N/S
			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC, 4): 
			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA, 3):
				if(l_rx_buffer[0] == 'S')
					l_current_data.Latitude = -l_current_data.Latitude;
				break;

			// Longitude
			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC, 5):
			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA, 4):
				l_current_data.Longitude = drvGPSConvertDegree(&pos, &success);
				break;

			// E/W
			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC, 6):
			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA, 5):
				if (l_rx_buffer[0] == 'W')
					l_current_data.Longitude = -l_current_data.Longitude;
				break;

			// Speed (GPRMC)
			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC, 7):
				strStringToFixedWord(l_rx_buffer, &pos, &success,	&l_current_data.Speed, 2, 100);
				break;

			// Course (GPRMC)
			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC, 8):
				strStringToFixedWord(l_rx_buffer, &pos, &success,	&l_current_data.Course, 2, 100);
				break;

			case GENERATE_CASE_ID(drvGPS_IDX_GPRMC, 9): // Date (GPRMC)
				l_current_data.UTCDateTime.Day = drvGPSConvertTwoDigitNumber(&pos, &success);
				l_current_data.UTCDateTime.Month = drvGPSConvertTwoDigitNumber(&pos, &success);
				l_current_data.UTCDateTime.Year = drvGPSConvertTwoDigitNumber(&pos, &success) + 2000;
				break;

			// Fix data (GPGGA)
			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA, 6):
					if( l_rx_buffer[0] > '0')
						l_current_data.Status = drvGPS_OK;
					else
						l_current_data.Status = drvGPS_NO_FIX;
					break;

			// Satellites used (GPGGA)
			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA, 7): 
				strStringToByte(l_rx_buffer, drvGPS_RX_BUFFER_LENGTH, &pos, &success,	&l_current_data.SatellitesUsed);
				break;

			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA, 8): // HDOP
				//strStringToFixedInt16(l_rx_buffer, drvGPS_RX_BUFFER_LENGTH, &pos, &success,	&l_current_data.HDOP);
				break;

			// Altitude (GPGGA)
			case GENERATE_CASE_ID(drvGPS_IDX_GPGGA, 9):
				//strStringToFixedInt16(l_rx_buffer, drvGPS_RX_BUFFER_LENGTH, &pos, &success,	&l_current_data.Altitude);
				break;
		}

		// continue parsing
		l_param_index++;
		l_rx_buffer_pos = 0;
	}

}

/*****************************************************************************/
/* Transmitter section                                                       */
/*****************************************************************************/

///////////////////////////////////////////////////////////////////////////////
// Character transmission interupt
void drvGPSUARTTransmitInterrupt(void)
{
}


#if 0

	dosByte new_push_pointer;

	// calculate buffer push pointer
	new_push_pointer = l_receive_push_pointer + 1;
	if( new_push_pointer >= drvGPSUART_RECEIVE_BUFFER_LENGTH )
		new_push_pointer = 0;
		
	// check available space
	if( new_push_pointer == l_receive_pop_pointer )
	{
		return;
	}	
	
	// check received character
	switch( in_char )
	{
		// skip LF
		case ASCII_LF:
			break;

		// handle line end (CR)
		case ASCII_CR:
		{
			krnlMessage* message;

			// terminate GPS sentence
			l_receive_buffer[new_push_pointer] = ASCII_NUL;

			// send message to initiate parsing
			message = krnlAllocateAsyncMessage();
					
			if( message != DOS_NULL )
				message->Id = msgGPS_SENTENCE_RECEIVED;
		}
		break;

		// store character
		default:
			l_receive_buffer[new_push_pointer] = in_char;
			break;
	}
	
	// update pointer
	l_receive_push_pointer = new_push_pointer;
}	

/*****************************************************************************/
/* Transmitter section                                                       */
/*****************************************************************************/

///////////////////////////////////////////////////////////////////////////////
// True if transmitter buffer is empty
dosBool krnlGPSUARTIsTransmitterEmpty(void)
{
	return (g_transmit_pop_pointer == g_transmit_push_pointer);
}

///////////////////////////////////////////////////////////////////////////////
// UART Transmit complete interrupt
void krnlGPSUARTTransmitInterrupt(void)
{
	// increment pointer
	g_transmit_pop_pointer++;
	
	if( g_transmit_pop_pointer >= GPSUART_TRANSMIT_BUFFER_LENGTH )
		g_transmit_pop_pointer = 0;
		
	// send character
	drvGPSUARTSendCharacter( g_transmit_buffer[g_transmit_pop_pointer] );
	
	// disable interrupt if there is no more character to send		
	if( g_transmit_pop_pointer == g_transmit_push_pointer )
	{
		drvGPSUARTDisableTransmitInterrupt();
  }
}	

///////////////////////////////////////////////////////////////////////////////
// Send string
dosByte krnlGPSUARTSendString( dosString in_string )
{
	dosByte index = 0;
	
	while( in_string[index] != '\0' )
	{
		krnlGPSUARTSendCharacter( in_string[index++] );
	}
	
	return dosTrue;
}	

///////////////////////////////////////////////////////////////////////////////
// Send character
void krnlGPSUARTSendCharacter( dosChar in_char )
{
	dosByte next_push_pointer = g_transmit_push_pointer + 1;
	
	if( next_push_pointer >= GPSUART_TRANSMIT_BUFFER_LENGTH )
		next_push_pointer = 0;
		
	// wait if buffer is full
	while( next_push_pointer == g_transmit_pop_pointer );
		
	// store character in buffer
	g_transmit_buffer[next_push_pointer] = in_char;
	
	g_transmit_push_pointer = next_push_pointer;
	
	drvGPSUARTEnableTransmitInterrupt();
}
#endif

/*****************************************************************************/
/* Local functions                                                           */
/*****************************************************************************/

///////////////////////////////////////////////////////////////////////////////
/// @brief Converts GPS degree value to a fixed point value
/// @param inout_buffer_pos Start position of the number string within the rx buffer
/// @param inout_success Success flag. Must be true before calling this function and will be set to true or false depending of the conversion success.
/// @return Converted value
dosInt32 drvGPSConvertDegree(dosStringLength* inout_buffer_pos, dosBool* inout_success)
{
	dosDWord left_of_decimal;
	dosDWord hundred1000ths_of_minute;
	dosDWord mult;
	dosStringLength buffer_pos = *inout_buffer_pos;

	// sanity check
	if(!(*inout_success))
		return 0;

	// parse digit before dot
	left_of_decimal = 0;
	while(charIsDigit(l_rx_buffer[buffer_pos]))
	{
		left_of_decimal = left_of_decimal * 10 + (l_rx_buffer[buffer_pos] - '0');
		buffer_pos++;
	}

	// check for decimal dot
	if(l_rx_buffer[buffer_pos] == '.')
		buffer_pos++;
	else
		*inout_success = dosFalse;

	// process fractional part
  mult = 10000;
  hundred1000ths_of_minute = (left_of_decimal % 100UL) * 100000UL;
	while(l_rx_buffer[buffer_pos])
  {
		hundred1000ths_of_minute += mult * (l_rx_buffer[buffer_pos] - '0');
		mult /= 10;
		buffer_pos++;
  }
 
	return (left_of_decimal / 100) * 1000000 + (hundred1000ths_of_minute + 3) / 6;
}

///////////////////////////////////////////////////////////////////////////////
/// @brief Converts two digit number to a decimal value
/// @param inout_buffer_pos Start position of the number string within the rx buffer
/// @param inout_success Success flag. Must be true before calling this function and will be set to true or false depending of the conversion success.
/// @return Converted value
static dosByte drvGPSConvertTwoDigitNumber(dosStringLength* inout_buffer_pos, dosBool* inout_success)
{
	dosChar ch;
	dosByte value = 0;
	dosStringLength buffer_pos = *inout_buffer_pos;

	if(!(*inout_success))
		return 0;

	// process first digit
	ch = l_rx_buffer[buffer_pos];
	if(charIsDigit(ch))
		value = (ch - '0') * 10;
	else
		*inout_success = dosFalse;
		
	// process second digit
	ch = l_rx_buffer[buffer_pos+1];
	if(charIsDigit(ch))
		value += (ch - '0');
	else
		*inout_success = dosFalse;

	*inout_buffer_pos += 2;

	return value;
}

///////////////////////////////////////////////////////////////////////////////
/// @brief Stores received character
/// @param in_char Character to store
/// @return dosTrue if character was stored, dosFalse if buffer overrun occured


///////////////////////////////////////////////////////////////////////////////
/// @brief Stores received character
/// @param in_char Character to store
/// @return dosTrue if character was stored, dosFalse if buffer overrun occured
